/*
 * 运动控制类任务，接收佗螺仪的消息，维持机器人前进方向，在决策任务的控制下
 * 执行－转向、变速这样的动作
 */
#include <devices_ext/promotion.h>
#include <leaf/searchline.h>
#include <leaf/walkspeed.h>
#include "tasks.h"
#include "mtiguide.h"
#include "tasks/choices_api.h"


Promotion xl, xr;
Promotion yl, yr;
MsgBox motion_mb;
static TimerMsg adj_tmr;
static int max_speed = 0;
static int min_speed = 0;
SpeedPair cur_speed;
/*+消息处理器*/
static void adjust_findline( pSysMsg m );
static void set_max_speed( pSysMsg m );
static void set_min_speed( pSysMsg m );
static void set_graduate_speed( pSysMsg m );
static void set_graduate_acc( pSysMsg m );
static SysMsgHandler handlers[ NR_MOTION_MID ] = {
		NULL,
		adjust_findline,
		set_max_speed,
		set_min_speed,
		set_graduate_speed,
		set_graduate_acc,
};
#include <leaf/sysmsghandler.h>
/*-消息处理器*/

/* 辅助函数 */
static void full_stop(void);
static uint8_t get_sensor( void )
{
	return PINC & 0x0F;
}

/* 主任务体 */
DEF_TASK( motion, 256, 0 )
{
	DDRC &= ~0x0F;
	PORTC |= 0x0F;
	walk_init( 0, 0, 1, 2 );


	/* 1. kd = 0, 由小到大的调节kp, 使得相应时间满足需要
	 * 2. 调节kd，使得震荡不太明显 */
	searchline_set_kp( 10 );
	searchline_set_kd( 0 );

	while( true ){
		pSysMsg m = wait_sys_msg( &motion_mb );
		if( (pTimerMsg) m == &adj_tmr ){
			adjust_findline( m );
			start_timer_msg( &motion_mb, &adj_tmr, 20 );
		} else {
			handle_sys_msg( m );
		}
	}
}

/*+ 消息响应器 */
/* 调整 */
static void adjust_findline( pSysMsg m )
{
	SpeedPair calc = searchline( get_sensor(), &cur_speed );
	set_cur_speed( &calc );
	cur_speed = calc;
}
/* 设置最大速度 */
static void set_max_speed( pSysMsg m )
{
	int max = m->data.val;//获取消息中的数据
	searchilne_set_max( max );
	if( max != 0 && max_speed == 0 ){
		adjust_findline( m );
		start_timer_msg( &motion_mb, &adj_tmr, 20 );
	} else {
		cancel_timer_msg( &motion_mb, &adj_tmr );
		full_stop();
	}
	max_speed = max;
	ack_sys_msg( m );
}
/* 设置最小速度 */
static void set_min_speed( pSysMsg m )
{
	int min = m->data.val;//获取消息中的数据
	min_speed = min;
	searchline_set_min( min );
	ack_sys_msg( m );
}
/* 设定前进方向 */

/* 设置渐变速度相关，可先不实现，参考旅客程序中的实现 */
static void set_graduate_speed( pSysMsg m )
{
	ack_sys_msg( m );
}
static void set_graduate_acc( pSysMsg m )
{
	ack_sys_msg( m );
}
/*- 消息响应器 */

/* 辅助函数 */
static void full_stop( void )
{
	prom_speed( &xl, 0 );
	prom_speed( &xr, 0 );
	prom_speed( &yl, 0 );
	prom_speed( &yr, 0 );
}


/*+ Motion任务对外接口 */
void post_motion_command( pSysMsg msg, MotionMid mid )
{
	msg->id = mid;
	send_sys_msg( &motion_mb, msg );
	wait_sys_msg_ack( msg );
}

void post_max_speed( pSysMsg msg, int val )
{
	msg->data.val = val;
	post_motion_command( msg, SetMaxSpeed );
}

void post_min_speed( pSysMsg msg, int val )
{
	msg->data.val = val;
	post_motion_command( msg, SetMinSpeed );
}

/*- Motion任务对外接口 */

